#include <AccelStepper.h>

struct Pulsante {
  int pin;
  int statoFiltrato;
  int ultimoStatoLettura;
  unsigned long ultimoTempoDebounce;
};

AccelStepper motori[4] = {
  AccelStepper(1, 2, 3), 
  AccelStepper(1, 4, 5), 
  AccelStepper(1, 6, 7), 
  AccelStepper(1, 8, 9)
};

Pulsante btnCW[4] = { {A0, HIGH, HIGH, 0}, {A2, HIGH, HIGH, 0}, {10, HIGH, HIGH, 0}, {12, HIGH, HIGH, 0} };
Pulsante btnCCW[4] = { {A1, HIGH, HIGH, 0}, {A3, HIGH, HIGH, 0}, {11, HIGH, HIGH, 0}, {13, HIGH, HIGH, 0} };

unsigned long timerSeriale = 0;

void setup() {
  Serial.begin(9600);
  
  for (int i = 0; i < 4; i++) {
    pinMode(btnCW[i].pin, INPUT_PULLUP);
    pinMode(btnCCW[i].pin, INPUT_PULLUP);
    
    // Configurazione Accelerazione e Velocità
    motori[i].setMaxSpeed(1000);     // Velocità massima (step/s)
    motori[i].setAcceleration(500);  // Accelerazione (step/s^2)
  }
}

void loop() {
  for (int i = 0; i < 4; i++) {
    int orario = leggiPulsanteFiltrato(btnCW[i]);
    int antiorario = leggiPulsanteFiltrato(btnCCW[i]);

    if (orario == LOW) {
      // Imposta un target molto lontano in avanti
      motori[i].moveTo(1000000); 
    } 
    else if (antiorario == LOW) {
      // Imposta un target molto lontano all'indietro
      motori[i].moveTo(-1000000);
    } 
    else {
      // Se nessun tasto è premuto, ordina una decelerazione fino a fermarsi
      // stop() calcola il punto di arresto in base all'accelerazione impostata
      motori[i].stop(); 
    }

    // run() gestisce i passi necessari per seguire la rampa di accelerazione
    motori[i].run(); 
  }

  if (millis() - timerSeriale >= 2000) {
    timerSeriale = millis();
    stampaStatoPulsanti();
  }
}

int leggiPulsanteFiltrato(Pulsante &p) {
  int lettura = digitalRead(p.pin);
  if (lettura != p.ultimoStatoLettura) p.ultimoTempoDebounce = millis();
  if ((millis() - p.ultimoTempoDebounce) > 50) p.statoFiltrato = lettura;
  p.ultimoStatoLettura = lettura;
  return p.statoFiltrato;
}

void stampaStatoPulsanti() {
  Serial.print("Velocita' attuale Motore A: ");
  Serial.println(motori[0].speed());
}